#include <gic.h>
#include <arch.h>
#include <sysregs.h>
#include <interrupts.h>
#include <arch/cpu.h>
#include <spinlock.h>
#include <fences.h>
#include <plat.h>

volatile gicd_t* gicd = (void*)GICD_BASE;
volatile gicr_t* gicr = (void*)GICR_BASE;

static spinlock_t gicd_lock;
static spinlock_t gicr_lock;

/* GIC-600 specific register offsets */
#define IIDR_MODEL_ARM_GIC_600      0x0200043bU
#define IIDR_MODEL_ARM_GIC_600AE    0x0300043bU

#define GICR_PWRR           0x24U
/* GICR_PWRR fields */
#define PWRR_RDPD_SHIFT     0
#define PWRR_RDAG_SHIFT     1
#define PWRR_RDGPD_SHIFT    2
#define PWRR_RDGPO_SHIFT    3
#define PWRR_RDPD           (1U << PWRR_RDPD_SHIFT)
#define PWRR_RDAG           (1U << PWRR_RDAG_SHIFT)
#define PWRR_RDGPD          (1U << PWRR_RDGPD_SHIFT)
#define PWRR_RDGPO          (1U << PWRR_RDGPO_SHIFT)

/*
 * Values to write to GICR_PWRR register to power redistributor
 * for operating through the core (PWRR.RDAG = 0)
 */
#define PWRR_ON             (0U << PWRR_RDPD_SHIFT)
#define PWRR_OFF            (1U << PWRR_RDPD_SHIFT)

static void gic600_wait_group_not_in_transit(void)
{
    uint32_t pwrr;

    do {
        pwrr = gicr[get_cpuid()].PWRR;

        /* Check group not transitioning: RDGPD == RDGPO */
    } while (((pwrr & PWRR_RDGPD) >> PWRR_RDGPD_SHIFT) !=
        ((pwrr & PWRR_RDGPO) >> PWRR_RDGPO_SHIFT));
}

static void gic600_pwr_on(void)
{
    do {    /* Wait until group not transitioning */
        gic600_wait_group_not_in_transit();

        /* Power on redistributor */
        gicr[get_cpuid()].PWRR = PWRR_ON;

        /*
         * Wait until the power on state is reflected.
         * If RDPD == 0 then powered on.
         */
    } while ((gicr[get_cpuid()].PWRR & PWRR_RDPD) != PWRR_ON);
}

static bool gicr_need_power_mgmt()
{
    uint32_t reg = gicr[get_cpuid()].IIDR;

    return (((reg & IIDR_MODEL_MASK) == IIDR_MODEL_ARM_GIC_600) ||
            ((reg & IIDR_MODEL_MASK) == IIDR_MODEL_ARM_GIC_600AE));
}

static inline uint64_t gic_num_irqs()
{
    uint32_t itlinenumber =
        bit_extract(gicd->TYPER, GICD_TYPER_ITLN_OFF, GICD_TYPER_ITLN_LEN);
    return 32 * itlinenumber + 1;
}

/*
 * Wait for register write pending
 * TODO: add timed wait
 */
static inline int gic_wait_rwp(uint32_t intid)
{
    uint32_t rwp_mask;

    if (intid < 32) {
        while(gicr[get_cpuid()].CTLR & GICR_CTLR_UWP);
    } else {
        while(gicd->CTLR & GICR_CTLR_UWP);
    }

    return 0;
}

/*
 * Wake up GIC redistributor.
 * clear ProcessorSleep and wait till ChildAsleep is cleared.
 * ProcessSleep to be cleared only when ChildAsleep is set
 * Check if redistributor is not powered already.
 */
static void gicr_enable(void)
{
    /*
     * The WAKER_PS_BIT should be changed to 0
     * only when WAKER_CA_BIT is 1.
     */
    if (!gicr[get_cpuid()].WAKER & GICR_ChildrenASleep_BIT)
        return;

    gicr[get_cpuid()].WAKER &= ~ GICR_ProcessorSleep_BIT;
    while(gicr[get_cpuid()].WAKER & GICR_ChildrenASleep_BIT);
}

static inline void gicc_init()
{
    /* Switch to SRE=1 */
    MSR(ICC_SRE_EL3, 0x9);

    /* Set priority mask to 0xff, binary point=0x7 */
    MSR(ICC_PMR_EL1, -1);
    MSR(ICC_BPR1_EL1, 0x7);

    /* Enable Group0/Group1 Secure interrupts */
    MSR(ICC_IGRPEN0_EL1, ICC_IGRPEN_EL1_ENB_BIT);
    MSR(ICC_IGRPEN1_EL3, ICC_IGRPEN_EL1_ENB_BIT);
    ISB();
}

static inline void gicr_init()
{
    gicr[get_cpuid()].ICENABLER0 = -1;
    gicr[get_cpuid()].ICPENDR0 = -1;
    gicr[get_cpuid()].ICACTIVER0 = -1;
    gic_wait_rwp(0);

    for (int i = 0; i < GIC_NUM_PRIO_REGS(GIC_CPU_PRIV); i++) {
        gicr[get_cpuid()].IPRIORITYR[i] = -1;
    }
}

void gic_cpu_init()
{
    if (gicr_need_power_mgmt()) {
        gic600_pwr_on();
    }

    gicr_init();
    gicr_enable();
    gicc_init();
}

void gicd_init()
{
    size_t int_num = gic_num_irqs();

    /* Bring distributor to known state */
    for (int i = GIC_NUM_PRIVINT_REGS; i < GIC_NUM_INT_REGS(int_num); i++) {
        /**
         * Make sure all interrupts are not enabled, non pending,
         * non active.
         */
        gicd->ICENABLER[i] = -1;
        gicd->ICPENDR[i] = -1;
        gicd->ICACTIVER[i] = -1;
    }

    /* wait for rwp on GICD */
    gic_wait_rwp(32);

    /* All interrupts have lowest priority possible by default */
    for (int i = GIC_CPU_PRIV; i < GIC_NUM_PRIO_REGS(int_num); i++)
        gicd->IPRIORITYR[i] = -1;

    /* No CPU targets for any interrupt by default */
    for (int i = GIC_CPU_PRIV; i < GIC_NUM_TARGET_REGS(int_num); i++)
        gicd->ITARGETSR[i] = 0;

    /* ICFGR are platform dependent, lets leave them as is */

    /* No need to setup gicd->NSACR as all interrupts are  setup to group 1 */

    /* Enable distributor and affinity routing */
    gicd->CTLR |= GICD_CTLR_ARE_NS_BIT | GICD_CTLR_EN_BIT;
    //gicd->CTLR |= 0x37;

    /* wait for pending write GICD_CTRL.RWP */
    while((gicd->CTLR >> 31) & 0x1);
}

void gic_init()
{
    if (get_cpuid() == 0) {
        gicd_init();
    }

    gic_cpu_init();
}


void gic_handle(void)
{
    unsigned long ack;
    uint64_t id;

    //TODO:only support G1S now
    /* Service all pending interrupts */
    while(1) {
        ack = MRS(ICC_IAR0_EL1);
        id = ack & ((1UL << 24) -1);

        if (id >= 1022) {
            /* no pending interrupt to this PE */
            return;
        }

        irq_handle(id);

        fence_sync();

        /* Clear the interrupt */
        MSR(ICC_EOIR0_EL1, ack);
    }
}

uint64_t gicd_get_prio(uint64_t int_id)
{
    uint64_t reg_ind = GIC_PRIO_REG(int_id);
    uint64_t off = GIC_PRIO_OFF(int_id);

    spin_lock(&gicd_lock);

    uint64_t prio =
        gicd->IPRIORITYR[reg_ind] >> off & BIT_MASK(off, GIC_PRIO_BITS);

    spin_unlock(&gicd_lock);

    return prio;
}

void gicd_set_icfgr(uint64_t int_id, uint8_t cfg)
{
    spin_lock(&gicd_lock);

    uint64_t reg_ind = (int_id * GIC_CONFIG_BITS) / (sizeof(uint32_t) * 8);
    uint64_t off = (int_id * GIC_CONFIG_BITS) % (sizeof(uint32_t) * 8);
    uint64_t mask = ((1U << GIC_CONFIG_BITS) - 1) << off;

    gicd->ICFGR[reg_ind] = (gicd->ICFGR[reg_ind] & ~mask) | ((cfg << off) & mask);

    spin_unlock(&gicd_lock);
}

void gicd_set_prio(uint64_t int_id, uint8_t prio)
{
    uint64_t reg_ind = GIC_PRIO_REG(int_id);
    uint64_t off = GIC_PRIO_OFF(int_id);
    uint64_t mask = BIT_MASK(off, GIC_PRIO_BITS);

    spin_lock(&gicd_lock);

    gicd->IPRIORITYR[reg_ind] =
        (gicd->IPRIORITYR[reg_ind] & ~mask) | ((prio << off) & mask);

    spin_unlock(&gicd_lock);
}

enum int_state gicd_get_state(uint64_t int_id)
{
    uint64_t reg_ind = GIC_INT_REG(int_id);
    uint64_t mask = GIC_INT_MASK(int_id);

    spin_lock(&gicd_lock);

    enum int_state pend = (gicd->ISPENDR[reg_ind] & mask) ? PEND : 0;
    enum int_state act = (gicd->ISACTIVER[reg_ind] & mask) ? ACT : 0;

    spin_unlock(&gicd_lock);

    return pend | act;
}

static void gicd_set_pend(uint64_t int_id, bool pend)
{
    spin_lock(&gicd_lock);

    if (gic_is_sgi(int_id)) {
        uint64_t reg_ind = GICD_SGI_REG(int_id);
        uint64_t off = GICD_SGI_OFF(int_id);

        if (pend) {
            gicd->SPENDSGIR[reg_ind] = (1U) << (off + get_cpuid());
        } else {
            gicd->CPENDSGIR[reg_ind] = BIT_MASK(off, 8);
        }
    } else {
        uint64_t reg_ind = GIC_INT_REG(int_id);

        if (pend) {
            gicd->ISPENDR[reg_ind] = GIC_INT_MASK(int_id);
        } else {
            gicd->ICPENDR[reg_ind] = GIC_INT_MASK(int_id);
        }
    }

    spin_unlock(&gicd_lock);
}

void gicd_set_act(uint64_t int_id, bool act)
{
    uint64_t reg_ind = GIC_INT_REG(int_id);

    spin_lock(&gicd_lock);

    if (act) {
        gicd->ISACTIVER[reg_ind] = GIC_INT_MASK(int_id);
    } else {
        gicd->ICACTIVER[reg_ind] = GIC_INT_MASK(int_id);
    }

    spin_unlock(&gicd_lock);
}

void gicd_set_state(uint64_t int_id, enum int_state state)
{
    gicd_set_act(int_id, state & ACT);
    gicd_set_pend(int_id, state & PEND);
}

void gicd_set_trgt(uint64_t int_id, uint8_t trgt)
{
    uint64_t reg_ind = GIC_TARGET_REG(int_id);
    uint64_t off = GIC_TARGET_OFF(int_id);
    uint32_t mask = BIT_MASK(off, GIC_TARGET_BITS);

    spin_lock(&gicd_lock);

    gicd->ITARGETSR[reg_ind] =
        (gicd->ITARGETSR[reg_ind] & ~mask) | ((trgt << off) & mask);

    spin_unlock(&gicd_lock);
}

void gicd_set_route(uint64_t int_id, uint64_t trgt)
{
    if (gic_is_priv(int_id)) return;

    gicd->IROUTER[int_id] = trgt;
}

void gicd_set_enable(uint64_t int_id, bool en)
{
    uint64_t bit = GIC_INT_MASK(int_id);

    uint64_t reg_ind = GIC_INT_REG(int_id);
    spin_lock(&gicd_lock);
    if (en)
        gicd->ISENABLER[reg_ind] = bit;
    else
        gicd->ICENABLER[reg_ind] = bit;
    spin_unlock(&gicd_lock);
}

void gicr_set_prio(uint64_t int_id, uint8_t prio, uint32_t gicr_id)
{
    uint64_t reg_ind = GIC_PRIO_REG(int_id);
    uint64_t off = GIC_PRIO_OFF(int_id);
    uint64_t mask = BIT_MASK(off, GIC_PRIO_BITS);

    spin_lock(&gicr_lock);

    gicr[gicr_id].IPRIORITYR[reg_ind] =
        (gicr[gicr_id].IPRIORITYR[reg_ind] & ~mask) | ((prio << off) & mask);

    spin_unlock(&gicr_lock);
}

uint64_t gicr_get_prio(uint64_t int_id, uint32_t gicr_id)
{
    uint64_t reg_ind = GIC_PRIO_REG(int_id);
    uint64_t off = GIC_PRIO_OFF(int_id);

    spin_lock(&gicr_lock);

    uint64_t prio =
        gicr[gicr_id].IPRIORITYR[reg_ind] >> off & BIT_MASK(off, GIC_PRIO_BITS);

    spin_unlock(&gicr_lock);

    return prio;
}

void gicr_set_icfgr(uint64_t int_id, uint8_t cfg, uint32_t gicr_id)
{
    spin_lock(&gicr_lock);

    uint64_t reg_ind = (int_id * GIC_CONFIG_BITS) / (sizeof(uint32_t) * 8);
    uint64_t off = (int_id * GIC_CONFIG_BITS) % (sizeof(uint32_t) * 8);
    uint64_t mask = ((1U << GIC_CONFIG_BITS) - 1) << off;

    if (reg_ind == 0) {
        gicr[gicr_id].ICFGR0 =
            (gicr[gicr_id].ICFGR0 & ~mask) | ((cfg << off) & mask);
    } else {
        gicr[gicr_id].ICFGR1 =
            (gicr[gicr_id].ICFGR1 & ~mask) | ((cfg << off) & mask);
    }

    spin_unlock(&gicr_lock);
}

enum int_state gicr_get_state(uint64_t int_id, uint32_t gicr_id)
{
    uint64_t mask = GIC_INT_MASK(int_id);

    spin_lock(&gicr_lock);

    enum int_state pend = (gicr[gicr_id].ISPENDR0 & mask) ? PEND : 0;
    enum int_state act = (gicr[gicr_id].ISACTIVER0 & mask) ? ACT : 0;

    spin_unlock(&gicr_lock);

    return pend | act;
}

static void gicr_set_pend(uint64_t int_id, bool pend, uint32_t gicr_id)
{
    spin_lock(&gicr_lock);
    if (pend) {
        gicr[gicr_id].ISPENDR0 = (1U) << (int_id);
    } else {
        gicr[gicr_id].ICPENDR0 = (1U) << (int_id);
    }
    spin_unlock(&gicr_lock);
}

void gicr_set_act(uint64_t int_id, bool act, uint32_t gicr_id)
{
    spin_lock(&gicr_lock);

    if (act) {
        gicr[gicr_id].ISACTIVER0 = GIC_INT_MASK(int_id);
    } else {
        gicr[gicr_id].ICACTIVER0 = GIC_INT_MASK(int_id);
    }

    spin_unlock(&gicr_lock);
}

void gicr_set_state(uint64_t int_id, enum int_state state, uint32_t gicr_id)
{
    gicr_set_act(int_id, state & ACT, gicr_id);
    gicr_set_pend(int_id, state & PEND, gicr_id);
}

void gicr_set_trgt(uint64_t int_id, uint8_t trgt, uint32_t gicr_id)
{
    spin_lock(&gicr_lock);

    spin_unlock(&gicr_lock);
}

void gicr_set_route(uint64_t int_id, uint8_t trgt, uint32_t gicr_id)
{
    gicr_set_trgt(int_id, trgt, gicr_id);
}

void gicr_set_enable(uint64_t int_id, bool en, uint32_t gicr_id)
{
    uint64_t bit = GIC_INT_MASK(int_id);

    spin_lock(&gicr_lock);
    if (en)
        gicr[gicr_id].ISENABLER0 = bit;
    else
        gicr[gicr_id].ICENABLER0 = bit;
    spin_unlock(&gicr_lock);
}

static bool irq_in_gicd(uint64_t int_id)
{
    if (int_id > 32 && int_id < 1025) return true;
    else return false;
}

void gic_send_sgi(uint64_t cpu_target, uint64_t sgi_num)
{
    if (sgi_num >= GIC_MAX_SGIS) return;

    //TODO: 这里是简陋的实现,后续要考虑多个cluster的情形
    uint64_t sgi = (1UL << (cpu_target & 0xffull)) | (sgi_num << 24);
    MSR(ICC_SGI0R_EL1, sgi);
}

void gic_set_prio(uint64_t int_id, uint8_t prio)
{
    if (irq_in_gicd(int_id)) {
        return gicd_set_prio(int_id, prio);
    } else {
        return gicr_set_prio(int_id, prio, get_cpuid());
    }
}

uint64_t gic_get_prio(uint64_t int_id)
{
    if (irq_in_gicd(int_id)) {
        return gicd_get_prio(int_id);
    } else {
        return gicr_get_prio(int_id, get_cpuid());
    }
}

void gic_set_icfgr(uint64_t int_id, uint8_t cfg)
{
    if (irq_in_gicd(int_id)) {
        return gicd_set_icfgr(int_id, cfg);
    } else {
        return gicr_set_icfgr(int_id, cfg, get_cpuid());
    }
}

enum int_state gic_get_state(uint64_t int_id)
{
    if (irq_in_gicd(int_id)) {
        return gicd_get_state(int_id);
    } else {
        return gicr_get_state(int_id, get_cpuid());
    }
}

static void gic_set_pend(uint64_t int_id, bool pend)
{
    if (irq_in_gicd(int_id)) {
        return gicd_set_pend(int_id, pend);
    } else {
        return gicr_set_pend(int_id, pend, get_cpuid());
    }
}

void gic_set_act(uint64_t int_id, bool act)
{
    if (irq_in_gicd(int_id)) {
        return gicd_set_act(int_id, act);
    } else {
        return gicr_set_act(int_id, act, get_cpuid());
    }
}

void gic_set_state(uint64_t int_id, enum int_state state)
{
    if (irq_in_gicd(int_id)) {
        return gicd_set_state(int_id, state);
    } else {
        return gicr_set_state(int_id, state, get_cpuid());
    }
}

void gic_set_trgt(uint64_t int_id, uint8_t trgt)
{
    if (irq_in_gicd(int_id)) {
        return gicd_set_trgt(int_id, trgt);
    } else {
        return gicr_set_trgt(int_id, trgt, get_cpuid());
    }
}

void gic_set_route(uint64_t int_id, uint64_t trgt)
{
    return gicd_set_route(int_id, trgt);
}

void gic_set_enable(uint64_t int_id, bool en)
{
    if (irq_in_gicd(int_id)) {
        return gicd_set_enable(int_id, en);
    } else {
        return gicr_set_enable(int_id, en, get_cpuid());
    }
}

